#include "cartographer/rcm/particle_filter/particle_filter.h"

namespace cartographer
{
    bool ParticleFilter::Initiate(const ParticleGenerateFunc &generator)
    {
        if (!generator || particle_num_ < 1)
            return false;
        weights.resize(particle_num_);
        particles.resize(particle_num_);
        for (int i = 0; i < particle_num_; ++i)
        {
            auto particle = generator();
            particle->id = i;
            particle->weight = 1.0;
            particles[i] = particle;
            weights[i] = particle->weight;
        }
        initialized_ = true;
        return true;
    }

    void ParticleFilter::UpdateWeight(const ParticleWeightFunc &weight)
    {
        double weights_sum = 0.0;
        for (int i = 0; i < particle_num_; ++i)
        {
            double weight = weight(particles[i]);
            weights_sum += weight;
            particles[i]->weight = weight;
        }
        for (int i = 0; i < particle_num_; i++)
        {
            particles[i]->weight /= weights_sum;
            weights[i] = particles[i]->weight;
        }
    }
} // namespace cartographer

//void ParticleFilter::DataAssociation(std::vector<LandmarkObs> predicted, std::vector<LandmarkObs> &observations)
//{
//    for (auto pred : predicted)
//    {
//        double dist_min = std::numeric_limits<double>::max();
//        for (auto observation : observations)
//        {
//            double distance = dist(observation.x, observation.y, pred.x, pred.y);
//            if (distance < dist_min)
//            {
//                observation.id = pred.id;
//            }
//            dist_min = distance;
//        }
//    }
//}
